#include "supercap.h"
#include "bsp_tim1_pwm.h"
#include "oled_display.h"
#include "bsp_oled.h"
#include "bsp_key.h"
#include "pid.h"
#include "bsp_delay.h"
#include "usart.h"
#include "stdio.h"
#define BOARD_ID                        0                           //板子编号
#define POWER_SET_DEFAULT               15.0f                       //上电默认功率

#define DISABLE_POST                    0                           //跳过上电检查
#define DISABLE_BUCKBOOST               0                           //关闭H桥
#define DISABLE_PROTECTION              0                           //关闭保护
#define IGNORE_CAN_ERROR                0                           //不显示CAN通信错误

int fputc(int ch, FILE *f)
{
	/* 发送一个字节数据到串口DEBUG_USART */
	HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 1000);	
	return (ch);
}
//最后一次CAN接收时间戳
uint32_t last_can_tick;
//状态码
uint32_t s_code = 0;

//0 in_v_bias, 1 in_v_ratio, 2 cap_v_bias, 3 cap_v_ratio, 4 cap_c_bias, 5 cap_c_ratio, 6 in_c_bias, 7 in_c_ratio, 8 out_c_bias, 9 out_c_ratio
const float samp_parameters[][10] = {
{V_BIAS, V_RATIO_5050, V_BIAS, V_RATIO_5050, I_BIAS, I_RATIO_A1_5M, I_BIAS0, I_RATIO_A1_10M, I_BIAS0, I_RATIO_A1_10M},
{V_BIAS, V_RATIO_5050, V_BIAS, V_RATIO_5050, I_BIAS, I_RATIO_A1_2M, I_BIAS, I_RATIO_A2_4M,  I_BIAS, I_RATIO_A2_4M},
{V_BIAS, V_RATIO_5050, V_BIAS, V_RATIO_5050, I_BIAS, I_RATIO_A1_2M, I_BIAS, I_RATIO_A1_10M, I_BIAS, I_RATIO_A1_10M},
{V_BIAS, V_RATIO_5050, V_BIAS, V_RATIO_5050, I_BIAS, I_RATIO_A1_2M, I_BIAS, I_RATIO_A1_10M, I_BIAS, I_RATIO_A1_10M},
{V_BIAS, V_RATIO_5050, V_BIAS, V_RATIO_5050, I_BIAS, I_RATIO_A1_2M, I_BIAS, I_RATIO_A1_10M, I_BIAS, I_RATIO_A1_10M},
};

//0 in_v_1, 1 in_v_2, 2 cap_v_1, 3 cap_v_2, 4 cap_c_1, 5 cap_c_2, 6 in_c_1, 7 in_c_2, 8 out_c_1, 9 out_c_2
const float calibration_parameters[][10] = {
{20.00f, 24.00f, 17.00f, 22.00f, 3.00f, 6.00f, 2.00f, 4.00f, 2.00f, 4.00f},
{19.80f, 23.80f, 16.79f, 21.75f, 3.00f, 6.00f, 1.98f, 4.01f, 2.00f-0.65f, 4.00f-0.65f},
{20.16f, 24.19f, 17.01f, 22.03f, 3.00f, 6.00f, 1.96f, 3.93f, 1.96f, 4.02f},
{19.98f, 23.98f, 17.01f, 22.04f, 3.00f, 6.00f, 1.96f, 3.94f, 1.96f, 4.01f},
{20.00f, 24.00f, 17.00f, 22.00f, 3.00f, 6.00f, 2.00f, 4.00f, 2.00f, 4.00f},
};


/**
 * @brief 电容配置初始化
 * @param samples 采样的数据初始化
 * @param powercontrols 功率控制初始化
 * @param id 
 */
static void super_Config_init(sample_t* samples,power_control_t* powercontrols,uint8_t id);

/**
 * @brief 超级电容上电之前的自我诊断
 * @param samples 采样的数据
 * @param powercontrols 功率控制
 * @return 自检的状态
 */
static uint32_t super_cap_ON_diagnose(sample_t* samples, power_control_t* powercontrols);

/**
 * @brief 计算剩余的容量和最大的电流
 * @param powercontrols 功率控制
 * @param states 电容的状态
 * @return 剩余量
 */
static uint32_t calc_cap_Remain_and_Current(power_control_t* powercontrols, status_t* states,sample_t* samples);

/**
 * @brief 电容运行时的状态诊断
 * @param samples 采样的数据
 * @param states 电容的状态
 * @param powercontrols 电容的功率控制
 * @return 错误码
 */
static uint32_t RUNNING_Diagnose(sample_t* samples, status_t* states,power_control_t* powercontrols);

/**
 * @brief 超级电容的初始化
 * @param  
 */
 
 
 
 uint16_t cnt=0;
void supercap_init(void) {
    delay_init();
    bsp_adc_init();
    HRtim1_init();
    super_Config_init(&sample, &power_control, BOARD_ID);
    HAL_TIM_Base_Start_IT(&htim7);
    display_init();
	
    can_init();     //can初始化
    //上电自检
#if !DISABLE_POST
    s_code |= super_cap_ON_diagnose(&sample, &power_control);
#endif
    //自检没有错误
    if ((s_code & ERR_MASK) == 0) {
        //开始PWM和循环定时器
#if !DISABLE_BUCKBOOST
        HRtim1_init();
      //  buzzer_init_bea();   //初始化叫声
			
#endif
    }
//    else {
//        while (1) {
//            buzzer_warn_bea(s_code);   //错误警告的声音
//        }
 //   }
}


  
/**
 * @brief 超级电容任务的执行管理
 * @param  
 */
CCMRAM void supercap_loop(void) {
    //按键扫描
    key_Scanf();
    //清空状态标志位
    static uint8_t i = 0;
    if (i++ > 10) {
       // display_loop();
        i = 0;
    }
		uint16_t buff[4]={121,232,23232,323};
		can_send_message(buff,0x201);
//		printf("%3.2f\n",sample.dcdc_c);
//		printf("%3.2f\n",sample.cap_v);
		printf("%3.2f\n",sample.dcdc_p);
//		printf("%3.2f\n",sample.in_p);
//		printf("%3.2f\n",sample.out_p);
//		cnt++;
//		if(cnt<2000)
//		{
//		power_control.dcdc_curr=0.5;
//		
//		}
//		else if(cnt>2000&&cnt<4000)
//		{
//		power_control.dcdc_curr=2.5;
//		}
//		else if(cnt>4000)
//		{
//		cnt=0;
//		
//		}
//		
		
		
		
		
    s_code &= ~STATUS_MASK;
    //计算剩余容量和最大电流
    s_code |= calc_cap_Remain_and_Current(&power_control, &status, &sample);
    //状态检测
    s_code |= RUNNING_Diagnose(&sample, &status, &power_control);
    //发生错误   //停止PWM和控制循环
    if ((s_code & ERR_MASK) != 0) {
#if !DISABLE_PROTECTION
        setHRTim1_Pwm_outputs(10, 5, 10);
        status.capmode = CAP_CUTOFF;
#endif
    }
}

/**
 * @brief 电容配置初始化
 * @param samples 采样的数据初始化
 * @param powercontrols 功率控制初始化
 * @param id 
 */
CCMRAM static void super_Config_init(sample_t* samples, power_control_t* powercontrols, uint8_t id) {
    
    samples->in_v_filter.raw_bias = samp_parameters[id][0];
    samples->in_v_filter.ratio = samp_parameters[id][1];
    samples->cap_v_filter.raw_bias = (int)samp_parameters[id][2];
    samples->cap_v_filter.ratio = samp_parameters[id][3];
    samples->cap_c_filter.raw_bias = (int)samp_parameters[id][4];
    samples->cap_c_filter.ratio = samp_parameters[id][5];
    samples->in_c_filter.raw_bias = (int)samp_parameters[id][6];
    samples->in_c_filter.ratio = samp_parameters[id][7];
    samples->out_c_filter.raw_bias = (int)samp_parameters[id][8];
    samples->out_c_filter.ratio = samp_parameters[id][9];

    //校准参数
    ADC_Linear_calibration_init(&samples->in_v_filter, INV_CAL_POINT_1, calibration_parameters[id][0], INV_CAL_POINT_2, calibration_parameters[id][1]);
    ADC_Linear_calibration_init(&samples->cap_v_filter, CAPV_CAL_POINT_1, calibration_parameters[id][2],  CAPV_CAL_POINT_2, calibration_parameters[id][3]);
    ADC_Linear_calibration_init(&samples->cap_c_filter, CAPC_CAL_POINT_1, calibration_parameters[id][4],  CAPC_CAL_POINT_2, calibration_parameters[id][5]);
    ADC_Linear_calibration_init(&samples->in_c_filter, INC_CAL_POINT_1, calibration_parameters[id][6], INC_CAL_POINT_2, calibration_parameters[id][7]);
    ADC_Linear_calibration_init(&samples->out_c_filter, OUTC_CAL_POINT_1, calibration_parameters[id][8], OUTC_CAL_POINT_2, calibration_parameters[id][9]);

    //控制参数
    powercontrols->cap_v_max = CAPV_MAX;     //电容最大允许充到的电压！！！
    powercontrols->cap_max_curr = ICAP_MAX;
    powercontrols->power_set = POWER_SET_DEFAULT;
    const static float C_PID[3] = { 0.8f, 0.15f, 0.1f };
    const static float V_PID[3] = { 1.0f, 0.06f, 3.0f };
    const static float P_PID[3] = { 1.5f, 0.05f, 2.52 };
    //PID初始化
    PID_init(&powercontrols->currout_loop, PID_DELTA, C_PID, 150, 25, -150, -25);     //电流环
    PID_init(&powercontrols->voltout_loop, PID_DELTA, V_PID, 100, 25, -100, -25);      //电压环
    PID_init(&powercontrols->powerin_loop, PID_POSITION, P_PID, 400, 50, -400, -50);       //输入功率环
}

/**
 * @brief 超级电容上电之前的自我诊断
 * @param samples 采样的数据
 * @param powercontrols 功率控制
 * @return 自检的状态
 */
CCMRAM static uint32_t super_cap_ON_diagnose(sample_t* samples, power_control_t* powercontrols) {
    uint32_t err_code = 0;
    //等待ADC数据
    while (!HAL_IS_BIT_SET(HAL_ADC_GetState(&hadc1), HAL_ADC_STATE_REG_EOC) && \
        !HAL_IS_BIT_SET(HAL_ADC_GetState(&hadc2), HAL_ADC_STATE_REG_EOC)) {
        delay_ms(50);
    }
    //采样10次
    for (uint8_t i = 0; i < 10; ++i) {
        ADC_Value_To_Elec(&samples->in_v_filter, (uint16_t)samples->adc2_buf[1]);
        ADC_Value_To_Elec(&samples->out_c_filter, (uint16_t)samples->adc1_buf[0]);
        ADC_Value_To_Elec(&samples->in_c_filter, (uint16_t)samples->adc1_buf[2]);
        ADC_Value_To_Elec(&samples->cap_c_filter, (uint16_t)samples->adc1_buf[1]);
        ADC_Value_To_Elec(&samples->cap_v_filter, (uint16_t)samples->adc2_buf[0]);
        delay_us(50);
    }
    samples->in_v = samples->in_v_filter.output;
    samples->out_c = samples->out_c_filter.output;
    samples->in_c = samples->in_c_filter.output;
    samples->cap_c = samples->cap_c_filter.output;
    samples->cap_v = samples->cap_v_filter.output;
    //检测过压
    if (samples->in_v_filter.output > INV_MAX) err_code |= ERR_OVER_VOLTAGE;

    //检测欠压
    if (samples->in_v_filter.output < INV_MIN) err_code |= ERR_LOW_VOLTAGE;

    //检测电流传感器异常
    if (fabsf(samples->in_c_filter.output) > 0.5f || fabsf(samples->out_c_filter.output) > 0.5f \
        || fabsf(samples->in_c_filter.output - samples->out_c_filter.output) > 0.5f) {
        err_code |= ERR_CURRENT_SENSOR;
    }
    return err_code;
}

/**
 * @brief 计算剩余的容量和最大的电流
 * @param powercontrols 功率控制
 * @param states 电容的状态
 * @return 剩余量
 */
CCMRAM static uint32_t calc_cap_Remain_and_Current(power_control_t* powercontrols, status_t* states, sample_t* samples) {
    uint32_t status_code = 0;

    //计算电容最大电流
    //为防止电容电压过低影响最大电流, 设定最大电流的最小值1A
    powercontrols->dcdc_max_curr = fmaxf((samples->cap_v / samples->dcdc_v) * (powercontrols->cap_max_curr - 0.0f), 1.0f);

    //计算电容剩余能量的百分比
    states->cap_percent = (int)(((samples->cap_v * samples->cap_v) / (CAPV_MAX * CAPV_MAX)) * 100.0f);
    if (states->cap_percent < 0) states->cap_percent = 0;
    if (states->cap_percent > 100) states->cap_percent = 100;

    //判断电容模式
    if (states->capmode == CAP_DISCHARGE) {
        if (states->cap_percent < 20) {
            //放电到20%以下处于低电量状态
            status_code = STATUS_CAP_LOW;
        }
        else {
            //正常放电
            status_code = STATUS_CAP_DISCHARGING;
        }
    }
    else if (states->capmode == CAP_CHARGE) {
        if (states->control_mode == CONTROL_CC) {
            //恒流可以认为没有充满
            status_code = STATUS_CAP_CHARGING;
        }
        else if (states->control_mode == CONTROL_CV) {
            //恒压可以认为已经充满
            status_code = STATUS_CAP_FULL;
        }
    }
    return status_code;
}

/**
 * @brief 电容运行时的状态诊断
 * @param samples 采样的数据
 * @param states 电容的状态
 * @param powercontrols 电容的功率控制
 * @return 错误码
 */
CCMRAM static uint32_t RUNNING_Diagnose(sample_t* samples, status_t* states, power_control_t* powercontrols) {
    //保护延时
    static uint16_t disable_cnt = 0;   uint32_t err_code = 0;
    //CAN通信异常
#if !IGNORE_CAN_ERROR
  //  if (HAL_GetTick() - last_can_tick > 1000) err_code |= ERR_CAN;
#endif

    //判断保护条件
    if (((states->control_mode == CONTROL_CC || samples->cap_v < 22.0f) && fabsf(samples->dcdc_c \
        - powercontrols->dcdc_curr) > 1.0f) || samples->in_v > INV_MAX || samples->in_v < INV_MIN || \
        samples->in_c > INC_MAX || samples->in_c < -INC_MAX) {
        disable_cnt++;
        if (disable_cnt > 50) {
            //欠压
            if (samples->in_v < INV_MIN) err_code |= ERR_LOW_VOLTAGE;
            //过压
            if (samples->in_v > INV_MAX) err_code |= ERR_OVER_VOLTAGE;
            //过流
            if (samples->in_c > INC_MAX || samples->in_c < -INC_MAX) err_code |= ERR_OVER_CURRENT;
            //充放电异常
            if ((states->control_mode == CONTROL_CC || samples->cap_v < 22.0f) && fabsf(samples->dcdc_c  \
                - powercontrols->dcdc_curr) > 1.0f) err_code |= ERR_BUCKBOOST;
        }
    }
    else {
        disable_cnt = 0;
    }
    return err_code;
}
